/*
 * @Description: TF动态广播器
 * @Author: BiChunkai 321521004@qq.com
 * @Date: 2025-06-11 16:18:56
 * @FilePath: /ros2_test/src/demo_cpp_tf/src/dynamic_tf_broadcaster.cpp
 * 
 * Copyright (c) 2025 by 无锡捷普迅科技有限公司, All Rights Reserved. 
 */
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp" // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"             // 提供四元数接口
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // 提供转换接口
#include "tf2_ros/transform_broadcaster.h"         // 提供动态广播器接口

using namespace std::chrono_literals;

class DynamicTFBroadcaster : public rclcpp::Node
{
public:
    DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster")
    {
        // 创建动态广播发布器
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        // 定时发布变换
        timer_ = this->create_wall_timer(500ms, std::bind(&DynamicTFBroadcaster::publish_tf, this));

    }

    void publish_tf()
    {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = this->get_clock()->now();
        transform.header.frame_id = "map";
        transform.child_frame_id = "base_link";
        transform.transform.translation.x = 2.0;
        transform.transform.translation.y = 3.0;
        transform.transform.translation.z = 0.0;

        tf2::Quaternion quat;
        quat.setRPY(0, 0, 30 * M_PI / 180);              // 弧度制欧拉角转四元数
        transform.transform.rotation = tf2::toMsg(quat); // 四元数转消息

        tf_broadcaster_->sendTransform(transform);       // 发布变换
    }

private:
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    rclcpp::TimerBase::SharedPtr timer_;
};


int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<DynamicTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}